/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/



#include "stdafx.h"
#include "Bullet车.h"








btRaycastVehicle::btVehicleTuning m_tuning;


static int rightIndex = 0;
static int upIndex = 1;
static int forwardIndex = 2;



S_Bullet车::S_Bullet车(S_Bullet物理引擎核心& core, const S_车体参数& 车体参数, uint32 key) {
	m_物理引擎枚举 = E_物理引擎::E_物理引擎_Bullet;
	m_物理体类型 = E_物理体类型::E_物理体_车辆;

	/*btTransform startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(btVector3(0, 0, 0));
	btCollisionShape* m_Shape = 0;
	float32 重量 = 1.0;

	switch (geom.m_Type) {
	case E_物理碰撞几何类型::E_碰撞几何_凸壳:
	{
		auto* g = dynamic_cast<S_Bullet凸包几何体*>(&geom);
		m_Shape = g->m_Shape;
		重量 = g->m_重量;
	}
	break;

	case E_物理碰撞几何类型::E_碰撞几何_方体:
	{
		auto* g = dynamic_cast<S_Bullet长方体几何体*>(&geom);
		m_Shape = g->m_Shape;
		重量 = g->m_重量;
	}
	break;
	default:
		return;
	}
	m_Shape->setMargin(0.03);*/



	m_Body = (btRigidBody*)(车体参数.m_车体刚体->f_getBody());

	m_vehicleRayCaster = new btDefaultVehicleRaycaster(core.m_物理域空间[0]);
	m_vehicle = new btRaycastVehicle(m_tuning, m_Body, m_vehicleRayCaster);

	m_Body->setActivationState(DISABLE_DEACTIVATION);
	//m_dynamicsWorld->addVehicle(m_vehicle);
	float	connectionHeight = 1.2f;
	bool	isFrontWheel = true;
	//choose coordinate system
	m_vehicle->setCoordinateSystem(rightIndex, upIndex, forwardIndex);
	//m_vehicle->setPitchControl(1.0);
	
	//btVector3 connectionPointCS0(CUBE_HALF_EXTENTS - (0.3 * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
	for (uint8 i = 0; i < 车体参数.m_车轮数量; ++i) {
		auto& 车轮 = 车体参数.m_车轮[i];

		btVector3 connectionPointCS0 = { 车轮.m_位置.x, 车轮.m_位置.y, 车轮.m_位置.z };
		btVector3 wheelDirectionCS0(车轮.m_悬挂压缩方向.x, 车轮.m_悬挂压缩方向.y, 车轮.m_悬挂压缩方向.z);
		btVector3 wheelAxleCS(-1, 0, 0);
		float32 悬挂长度 = 0.6;

		if (isFrontWheel == true) isFrontWheel = false;

		m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, 悬挂长度, 车轮.m_半径, m_tuning, isFrontWheel);
		
	}

	
	//connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS + (0.3 * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
	/*m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
	connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS + (0.3 * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
	isFrontWheel = false;
	m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
	connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS - (0.3 * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
	m_vehicle->addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);*/

	for (int i = 0; i < m_vehicle->getNumWheels(); i++) {
		auto& 车轮 = 车体参数.m_车轮[i];

		btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
		wheel.m_suspensionStiffness = 车轮.m_悬挂刚度;
		wheel.m_wheelsDampingRelaxation = 车轮.m_悬挂阻尼;
		wheel.m_wheelsDampingCompression = 车轮.m_悬挂压缩;
		wheel.m_frictionSlip = 车轮.m_轮胎摩擦;
		wheel.m_rollInfluence = 车轮.m_旋转影响;
		
	}

	//btTriangleMeshShape()
	//btRayAabb();
}

S_Bullet车::~S_Bullet车() {

}



void S_Bullet车::f_绑定关联对象(void* data, uint32 id)
{
	if (id >= m_车轮对象.size()) {
		m_车轮对象.push_back(data);
	}
	else {
		m_车轮对象[id] = data;
	}
}

void S_Bullet车::f_setLocRot(const vec3& loc, const vec3& r) {
	vec4 q = f_graph_欧拉角转四元数(r);

	btTransform sphereTrans;
	sphereTrans.setIdentity();
	sphereTrans.setOrigin(btVector3(loc.x, loc.y, loc.z));
	sphereTrans.setRotation({ q.x, q.y, q.z, q.w });
	m_Body->setWorldTransform(sphereTrans);
}

void S_Bullet车::f_setPos(const vec3& l) {

}

void S_Bullet车::f_setRot(const vec3& r) {

}

void S_Bullet车::f_setVel(const vec3& l) {
	m_Body->setAngularVelocity({ 0.0,0.0,0.0 });
	m_Body->setLinearVelocity({ l.x, l.y, l.z });
}

vec3 S_Bullet车::f_getLocation() {

	return vec3();
}

vec3 S_Bullet车::f_getLoc() {
	auto co = m_vehicle->getChassisWorldTransform().getOrigin();
	return {co.x(), co.y(), co.z()};
}

vec3 S_Bullet车::f_getVel() {
	auto s = m_Body->getLinearVelocity();
	//auto s = m_vehicle->getCurrentSpeedKmHour();
	return {s.x(), s.y(), s.z()};
}

vec4 S_Bullet车::f_get速度() {
	auto s = m_Body->getLinearVelocity();
	return { s.x(), s.y(), s.z(), m_vehicle->getCurrentSpeedKmHour() };
}

vec3 S_Bullet车::f_getRotVel() {
	auto s = m_Body->getAngularVelocity();
	return { s.x(), s.y(), s.z() };
}



void S_Bullet车::f_加速(const float32 s) {
	int wheelIndex = 2;
	m_vehicle->applyEngineForce(s, wheelIndex);
	
	wheelIndex = 3;
	m_vehicle->applyEngineForce(s, wheelIndex);

}

void S_Bullet车::f_倒车(const float32 s) {
	m_vehicle->applyEngineForce(s, 2);
	m_vehicle->applyEngineForce(s, 2);
}

void S_Bullet车::f_刹车(const float32 s) {
	m_vehicle->setBrake(s, 0);
	m_vehicle->setBrake(s, 1);
	m_vehicle->setBrake(s, 2);
	m_vehicle->setBrake(s, 3);
}

void S_Bullet车::f_手刹(const float32 s) {
	m_vehicle->setBrake(s, 2);
	m_vehicle->setBrake(s, 3);
}

void S_Bullet车::f_转向(const float32 s)
{
	int32 wheelIndex = 0;
	m_vehicle->setSteeringValue(s, wheelIndex);
	wheelIndex = 1;
	m_vehicle->setSteeringValue(s, wheelIndex);
}





